#include "encoder.h"

#include "stm32f10x.h"                  // Device header

int32_t sampleCodeTo_TIM2 = 0;
int16_t sampleSpeedTo_TIM2 = 0;

int32_t sampleCodeTo_TIM3 = 0;
int16_t sampleSpeedTo_TIM3 = 0;

int32_t sampleCodeTo_TIM4 = 0;
int16_t sampleSpeedTo_TIM4 = 0;

/**
 * @brief 
 * 
 * @param EncoderX 
 * @param code 采样器回调
 * @param speed 采样器回调
 * @return int 
 */
int CODER_Init(EncoderX *EncoderX, int32_t* *code, int16_t* *speed) {

  GPIO_InitTypeDef GIPO_InitStructure;
  GIPO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
  GIPO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;

  // Init RCC and GPIO
  if (EncoderX->encode_TIMx == TIM2)
  {
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

    GIPO_InitStructure.GPIO_Pin = GPIO_Pin_0 || GPIO_Pin_1;
    GPIO_Init(GPIOA, &GIPO_InitStructure);

    *code = &sampleCodeTo_TIM2;
    *speed = &sampleSpeedTo_TIM2; // 回调指针链接。回调地址给指针MotorX.code和MotorX.speed，以便其随时进行读取操作
  }
  else if (EncoderX->encode_TIMx == TIM3)
  {
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE);

    GIPO_InitStructure.GPIO_Pin = GPIO_Pin_6 || GPIO_Pin_7;
    GPIO_Init(GPIOA, &GIPO_InitStructure);

    *code = &sampleCodeTo_TIM3;
    *speed = &sampleSpeedTo_TIM3;
  }
  else if (EncoderX->encode_TIMx == TIM4)
  {
    RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM4, ENABLE);
    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);

    GIPO_InitStructure.GPIO_Pin = GPIO_Pin_6 || GPIO_Pin_7;
    GPIO_Init(GPIOB, &GIPO_InitStructure);

    *code = &sampleCodeTo_TIM4;
    *speed = &sampleSpeedTo_TIM4;
  }
  else
  {
    return -1; //函数执行错误返回
  }

  TIM_TimeBaseInitTypeDef TIM_TimeBaseInitStructure;
  TIM_TimeBaseStructInit(&TIM_TimeBaseInitStructure);
  TIM_TimeBaseInit(EncoderX->encode_TIMx, &TIM_TimeBaseInitStructure);

  TIM_EncoderInterfaceConfig(EncoderX->encode_TIMx, TIM_EncoderMode_TI12, TIM_ICPolarity_BothEdge, TIM_ICPolarity_BothEdge);
  TIM_ICInitTypeDef TIM_ICInitStruture;
  TIM_ICStructInit(&TIM_ICInitStruture);
  TIM_ICInitStruture.TIM_ICPolarity = TIM_ICPolarity_BothEdge;
  TIM_ICInitStruture.TIM_ICFilter = 8;
  TIM_ICInit(EncoderX->encode_TIMx, &TIM_ICInitStruture);

  TIM_ClearFlag(EncoderX->encode_TIMx, TIM_FLAG_Update);
  TIM_ITConfig(EncoderX->encode_TIMx, TIM_IT_Update, ENABLE);
  TIM_SetCounter(EncoderX->encode_TIMx, 0);
  TIM_Cmd(EncoderX->encode_TIMx, ENABLE);

  return 0;
}

/**
 * @param sampleTime_s 采样器固定在TIM1，最小采样周期 0.000001sec
 * 
 */
void MOTsampler_Init(float sampleTime_s) {

  /* Sampler init. 采样器固定在TIM1 */
  RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM1, ENABLE);
	TIM_InternalClockConfig(TIM1);
	
	TIM_TimeBaseInitTypeDef SAM_TIMTimeBaseInitStructure;
	SAM_TIMTimeBaseInitStructure.TIM_ClockDivision = TIM_CKD_DIV1;
	SAM_TIMTimeBaseInitStructure.TIM_CounterMode = TIM_CounterMode_Up;
	SAM_TIMTimeBaseInitStructure.TIM_Period  = sampleTime_s * 1000000-1;
	SAM_TIMTimeBaseInitStructure.TIM_Prescaler  = 72-1;
	SAM_TIMTimeBaseInitStructure.TIM_RepetitionCounter = 0;
	TIM_TimeBaseInit(TIM1,&SAM_TIMTimeBaseInitStructure);
	
	TIM_ClearFlag(TIM1, TIM_FLAG_Update);
	TIM_ITConfig(TIM1, TIM_IT_Update,ENABLE);

  NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);

  NVIC_InitTypeDef NVIC_InitStructure;
  NVIC_InitStructure.NVIC_IRQChannel = TIM1_UP_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 1;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;
	NVIC_Init(&NVIC_InitStructure);

  TIM_Cmd(TIM1, ENABLE);

}

/* Sampler */
void TIM1_UP_IRQHandler(void) {

  if(TIM_GetITStatus(TIM1,TIM_IT_Update)==SET)
  {
    sampleSpeedTo_TIM2 = (short)TIM2->CNT;
    sampleCodeTo_TIM2 += sampleSpeedTo_TIM2;
    TIM2->CNT = 0;

    sampleSpeedTo_TIM3 = (short)TIM3->CNT;
    sampleCodeTo_TIM3 += sampleSpeedTo_TIM3;
    TIM3->CNT = 0;

    sampleSpeedTo_TIM4 = (short)TIM4->CNT;
    sampleCodeTo_TIM4 += sampleSpeedTo_TIM4;
    TIM4->CNT = 0;
	
    TIM_ClearITPendingBit(TIM1,TIM_IT_Update);
  }
}
